#ifndef __CHASSIS_DRIVER_H_
#define __CHASSIS_DRIVER_H_

#include <memory>
#include <inttypes.h>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <deque>  // 添加deque支持

#include <iostream>
#include <string.h>
#include <string>
#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <unistd.h>
#include <rcl/types.h>
#include <sys/stat.h>

// 跨平台socket支持
#ifdef _WIN32
    #include <winsock2.h>
    #include <ws2tcpip.h>
    #pragma comment(lib, "ws2_32.lib")
    typedef SOCKET SocketType;
#else
    #include <sys/socket.h>
    #include <netinet/in.h>
    #include <arpa/inet.h>
    #include <unistd.h>
    typedef int SocketType;
    #define INVALID_SOCKET -1
    #define SOCKET_ERROR -1
    #define closesocket close
#endif

#include <fcntl.h>
#include <stdbool.h>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/float32.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/msg/odometry.hpp>

// 常量定义
constexpr uint8_t FRAME_HEADER = 0x7B;  // '{'
constexpr uint8_t FRAME_TAIL = 0x7D;    // '}'
constexpr size_t RECEIVE_DATA_SIZE = 10;
constexpr size_t SEND_DATA_SIZE = 10;

class ChassisDriver : public rclcpp::Node
{
public:
    ChassisDriver();
    ~ChassisDriver();
    void runLoop();

private:
    // 常量定义
    static const int FILTER_SIZE = 5;  // 滤波器窗口大小
    static constexpr int MAX_RETRY_COUNT = 3;        // 最大重试次数
    static constexpr int RETRY_DELAY_MS = 100;       // 重试延迟(毫秒)

    // 结构体定义
    struct RobotPose {
        double x;
        double y;
        double yaw;
        double last_vx;    // 上一次的线速度
        double last_vy;    // 上一次的横向速度
        double last_vtheta;// 上一次的角速度
    };

    struct RobotVelocity {
        double linear_x;
        double linear_y;
        double angular_z;
    };

    // 数据缓冲区 - 放在最前面，因为它们在构造函数中需要最先初始化
    uint8_t send_buffer_[SEND_DATA_SIZE];
    uint8_t receive_buffer_[RECEIVE_DATA_SIZE];

    // 基础成员变量
    bool socket_initialized_;
    bool is_connected_;
    SocketType udp_socket_;
    int receive_timeout_ms_;
    double sampling_time_;
    double battery_voltage_;
    rclcpp::Time last_update_time_;

    // 机器人状态
    RobotPose robot_pose_;
    RobotVelocity robot_velocity_;

    // 滤波相关
    std::deque<double> vx_history_;    // x方向速度历史
    std::deque<double> vy_history_;    // y方向速度历史
    std::deque<double> vtheta_history_;// 角速度历史
    double last_vx_;
    double last_vy_;
    double last_vw_;

    // 通信相关
    struct sockaddr_in remote_addr_;    // 远程地址
    struct sockaddr_in local_addr_;     // 本地地址
    std::string remote_ip_;             // 远程IP
    int remote_port_;                   // 远程端口
    int local_port_;                    // 本地端口

    // 配置参数
    std::string odom_frame_id_;
    std::string robot_frame_id_;
    std::string cmd_vel_topic_;

    // ROS发布者和订阅者
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr voltage_publisher_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    // 核心功能函数
    bool waitData();
    void publishOdometry();
    void publishVoltage();
    void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
    void updateRobotPose();
    void publishRobotState();

    // 初始化函数
    void initializeParameters();
    void initializePublishers();
    void initializeSubscribers();

    // UDP通信相关
    bool initSocket();
    bool sendData(const uint8_t* data, size_t len);
    bool receiveData(uint8_t* data, size_t len);
    void closeSocket();

    // 辅助函数
    double normalizeAngle(double angle);
    uint8_t calculateChecksum(const uint8_t* data, size_t len);
    double calculateMovingAverage(std::deque<double>& history, double new_value);
    void updateVelocityHistory(double vx, double vy, double vtheta);
    void calculateKinematics(double vx, double vy, double vtheta, double dt,
                           double& delta_x, double& delta_y, double& delta_theta);
};

#endif
